#ifndef CHaptik3dofPointer_H
#define CHaptik3dofPointer_H

#include "CGenericDevice.h"
#include "CGenericTool.h"
#include "CHaptikDevice.h"
#include "CColor.h"
#include "CProxyPointForceAlgo.h"
#include "CPotentialFieldForceAlgo.h"

typedef enum {
  RENDER_PROXY=0,RENDER_DEVICE,RENDER_PROXY_AND_DEVICE
} proxy_render_modes;

typedef enum {
  DEVICE_NAME=0,DEVICE_MODEL,DEVICE_MANUFACTURER
} device_features;

class cHaptik3dofPointer : public cGenericTool
{
  public:

    cHaptik3dofPointer(cWorld* a_world);

    virtual ~cHaptik3dofPointer();

    //OpenGL rendering
    virtual void render(const int a_renderMode=0);

    //Visualize frame
    virtual inline void visualizeFrames(const bool& a_showToolFrame) { m_showToolFrame = a_showToolFrame; }

    //Set ToolFrame characteristics
    virtual void setToolFrame(const bool& a_showToolFrame, const double& a_toolFrameSize);
    
	//Set wich device-representation render (device, proxy or both)
    virtual inline void setRenderingMode(const proxy_render_modes & render_mode) { m_render_mode = render_mode; }

	//Open the connection to Haptik Library and refresh the device list
	int startHaptikLibrary();
	//Get the selected device (if available)
	int getHaptikDevice(UINT32 a_deviceInput);

	//Get from Haptik Library informations about the devices connected
	const Char8* readDeviceInfoFromLibrary(int a_in, int a_char);
	UINT32 getNumberOfDevices() { return m_device->m_numDevices; };
	//Read the device info
	const Char8* readDeviceInfo(int a_char);

	//Get the id of the connected device
    UINT32 getPhysicalDevice() const { return m_physicalDevice; }
	//Get error type
	int getErrorType() const {return m_device->m_returnType; }
	//Control if my device is the default device
	int isDefaultDevice() const {return m_device->m_defaultDevice; }
	//Control if the system is available
	bool isSystemAvailable() const {return m_systemAvailable; }

	//Read the result string from device
	const Char8* getResultToString(int a_result);

	//Init device
    virtual int initialize(const bool a_resetEncoders = false);
    //Init communication with device
    virtual int start();
    //Stop communication with device
    virtual int stop();
	//Release the interface of the device
	void releaseInterface();
    //Enable force-feedback
    virtual int setForcesON();
    //Disable force-feedback
    virtual int setForcesOFF();

    //Update position of device
    virtual void updatePose();
    //Compute forces
    virtual void computeForces();
    //Apply computed forces to device
    virtual void applyForces();

	//Send a command to device and read the switch 0
	int buttonStateZero();

    //Get scale factor 
    virtual cVector3d getWorkspaceScaleFactors();

    //Set radius of the represented device
    virtual void setRadius(const double& a_radius);
    //Set/get device
    virtual void setDevice(cGenericDevice *a_device);
    virtual cGenericDevice* getDevice() { return m_device; }

    //Get function list of the proxy algorithms
    virtual cProxyPointForceAlgo* getProxy();

    //Set virtual workspace of the tool
    virtual void setWorkspace(const double& a_workspaceAxisX, const double& a_workspaceAxisY,
                      const double& a_workspaceAxisZ);

    //Normalized positions or not
    virtual void useNormalizedPositions(const bool& a_useNormalizedPositions)
      { m_useNormalizedPositions = a_useNormalizedPositions; }
    //Use or not of the normalized positions
    virtual bool getNormalizedPositionsEnabled() { return m_useNormalizedPositions; } 

    //Device/proxy/line colors
    cColorf m_colorDevice;
    cColorf m_colorProxy;
    cColorf m_colorProxyButtonOn;
    cColorf m_colorLine;

    //Axis orientation in local coordinates
    cMatrix3d m_deviceLocalRot;
    //Axis orientation in global coordinates
    cMatrix3d m_deviceGlobalRot;

    //Wait for a small force before init
    bool m_waitForSmallForce;

    //Vector of available force algorithms
    std::vector<cGenericPointForceAlgo*> m_pointForceAlgos;

    //Width workspace (Ignored when m_useNormalizedPositions is false)
    double m_halfWorkspaceAxisX;
    //Height workspace   (Ignored when m_useNormalizedPositions is false)
    double m_halfWorkspaceAxisY;
    //Depth workspace (Ignored when m_useNormalizedPositions is false)
    double m_halfWorkspaceAxisZ;

    //Device position in local coordinates
    cVector3d m_deviceLocalPos;
    //Device position in global coordinates
    cVector3d m_deviceGlobalPos;
    //Device velocity in device coordinates
    cVector3d m_deviceLocalVel;
    //Device velocity into the world
    cVector3d m_deviceGlobalVel;

    //Last computed force (global coordinates)
    cVector3d m_lastComputedGlobalForce;

    //Last computed force (local coordinates)
    cVector3d m_lastComputedLocalForce;

    //Memorize the workspace scale for every coordinate
	cVector3d m_workspaceScale;

	//Control variables
	int m_hapticsEnabled;
	int m_toolReady;
	int m_deviceEnabled;
	bool m_toolInPause;

    protected:

	//Assign the device to our main device
	void assignDevice();

	//Connected device
	cHaptikDevice* m_device;
    
	//Radius of the represented sphere
    double m_displayRadius;
    //General world
    cWorld* m_world;
    //Proxy frame
    bool m_showToolFrame;
    //Size frame proxy
    double m_toolFrameSize;
    //Set device-representation to render (device, proxy, entrambi)
    proxy_render_modes m_render_mode;
    //Use normalized positions or not
    bool m_useNormalizedPositions;
    //Force-feedback enabled or not
    bool m_forceON;
    //Init force-feedback (after SMALL FARCE)
    bool m_forceStarted;
    
	//Id physical device connected
	UINT32 m_physicalDevice;

	//Sistem available or not
	bool m_systemAvailable;
};

#endif